#include "user_lib.h"
#include "arm_math.h"

#ifdef _CMSIS_OS_H
#define user_malloc pvPortMalloc
#else
#define user_malloc malloc
#endif

/**
  * @brief          快速开方
  * @author         RM
  * @param          num 输入数据
  * @retval         输出结果
  */
fp32 invSqrt(fp32 num)
{
    fp32 halfnum = 0.5f * num;
    fp32 y = num;
    long i = *(long *)&y;
    i = 0x5f3759df - (i >> 1);
    y = *(fp32 *)&i;
    y = y * (1.5f - (halfnum * y * y));
    return y;
}

/**
  * @brief          斜波函数初始化
  * @author         RM
  * @param          ramp_source_type 斜波函数结构体
  * @param          frame_period 间隔的时间，单位 s
  * @param          max 最大值
  * @param          min 最小值
  * @retval         返回空
  */
void ramp_init(ramp_function_source_t *ramp_source_type, fp32 frame_period, fp32 max, fp32 min)
{
    ramp_source_type->frame_period = frame_period;
    ramp_source_type->max_value = max;
    ramp_source_type->min_value = min;
    ramp_source_type->input = 0.0f;
    ramp_source_type->out = 0.0f;
}

/**
  * @brief          斜波函数计算，根据输入的值进行叠加， 输入单位为 /s 即一秒后增加输入的值
  * @author         RM
  * @param          ramp_source_type 斜波函数结构体
  * @param          input 滤波参数
  * @retval         返回空
  */
void ramp_calc(ramp_function_source_t *ramp_source_type, fp32 input)
{
    ramp_source_type->input = input;
    ramp_source_type->out += ramp_source_type->input * ramp_source_type->frame_period;
    if (ramp_source_type->out > ramp_source_type->max_value)
    {
        ramp_source_type->out = ramp_source_type->max_value;
    }
    else if (ramp_source_type->out < ramp_source_type->min_value)
    {
        ramp_source_type->out = ramp_source_type->min_value;
    }
}

/**
  * @brief          一阶低通滤波初始化
  * @author         RM
  * @param          first_order_filter_type 一阶低通滤波结构体
  * @param          frame_period 间隔的时间，单位 s
  * @param          num 滤波参数
  * @retval         返回空
  */
void first_order_filter_init(first_order_filter_type_t *first_order_filter_type, fp32 frame_period, const fp32 num[1])
{
    first_order_filter_type->frame_period = frame_period;
    first_order_filter_type->num[0] = num[0];
    first_order_filter_type->input = 0.0f;
    first_order_filter_type->out = 0.0f;
}

/**
  * @brief          一阶低通滤波计算
  * @author         RM
  * @param          first_order_filter_type 一阶低通滤波结构体
  * @param          input 间隔的时间，单位 s
  * @retval         返回空
  */
void first_order_filter_cali(first_order_filter_type_t *first_order_filter_type, fp32 input)
{
    first_order_filter_type->input = input;
    first_order_filter_type->out =
    first_order_filter_type->num[0] / (first_order_filter_type->num[0] + first_order_filter_type->frame_period) * first_order_filter_type->out + first_order_filter_type->frame_period / (first_order_filter_type->num[0] + first_order_filter_type->frame_period) * first_order_filter_type->input;
}

/**
  * @brief          绝对限制
  * @author         RM
  * @param          num 输入参数指针
  * @param          Limit 限制
  * @retval         返回空
  */
void abs_limit(fp32 *num, fp32 Limit)
{
    if (*num > Limit)
    {
        *num = Limit;
    }
    else if (*num < -Limit)
    {
        *num = -Limit;
    }
}

/**
  * @brief          判断符号位
  * @author         RM
  * @param          value 输入
  * @retval         返回正负
  */
fp32 sign(fp32 value)
{
    if (value >= 0.0f)
    {
        return 1.0f;
    }
    else
    {
        return -1.0f;
    }
}

/**
  * @brief          浮点死区
  * @author         RM
  * @param          Value 输入值
  * @param          minValue 最小值
  * @param          maxValue 最大值
  * @retval         返回处理后的值。如果输入值在 `minValue` 和 `maxValue` 之间，返回 0.0f，否则返回原始值
  */
fp32 fp32_deadline(fp32 Value, fp32 minValue, fp32 maxValue)
{
    if (Value < maxValue && Value > minValue)
    {
        Value = 0.0f;
    }
    return Value;
}

/**
  * @brief          int16死区
  * @author         RM
  * @param          Value 输入值
  * @param          minValue 最小值
  * @param          maxValue 最大值
  * @retval         返回处理后的值。如果输入值在 `minValue` 和 `maxValue` 之间，返回 0，否则返回原始值
  */
int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue)
{
    if (Value < maxValue && Value > minValue)
    {
        Value = 0;
    }
    return Value;
}

/**
  * @brief          浮点限幅函数
  * @author         RM
  * @param          Value 输入值
  * @param          minValue 最小值
  * @param          maxValue 最大值
  * @retval         返回限制后的值。如果输入值小于 `minValue`，返回 `minValue`；如果大于 `maxValue`，返回 `maxValue`；否则返回原始值
  */
fp32 fp32_constrain(fp32 Value, fp32 minValue, fp32 maxValue)
{
    if (Value < minValue)
        return minValue;
    else if (Value > maxValue)
        return maxValue;
    else
        return Value;
}

/**
  * @brief          int16限幅函数
  * @author         RM
  * @param          Value 输入值
  * @param          minValue 最小值
  * @param          maxValue 最大值
  * @retval         返回限制后的值。如果输入值小于 `minValue`，返回 `minValue`；如果大于 `maxValue`，返回 `maxValue`；否则返回原始值
  */
int16_t int16_constrain(int16_t Value, int16_t minValue, int16_t maxValue)
{
    if (Value < minValue)
        return minValue;
    else if (Value > maxValue)
        return maxValue;
    else
        return Value;
}

/**
  * @brief          循环限幅函数，将输入值限制在指定的最小值和最大值之间，如果超出范围则循环回绕
  * @author         RM
  * @param          Input 输入值
  * @param          minValue 最小值
  * @param          maxValue 最大值
  * @retval         返回循环后的值。如果 `Input` 超出范围，则将其限制在 `[minValue, maxValue]` 范围内，超出的部分会从最小值或最大值回绕
  */
fp32 loop_fp32_constrain(fp32 Input, fp32 minValue, fp32 maxValue)
{
    if (maxValue < minValue)
    {
        return Input;
    }

    if (Input > maxValue)
    {
        fp32 len = maxValue - minValue;
        while (Input > maxValue)
        {
            Input -= len;
        }
    }
    else if (Input < minValue)
    {
        fp32 len = maxValue - minValue;
        while (Input < minValue)
        {
            Input += len;
        }
    }
    return Input;
}

/**
  * @brief          将角度值限制在 -180 到 180 度之间
  * @author         RM
  * @param          Ang 输入角度值
  * @retval         返回格式化后的角度值，确保其在 [-180.0f, 180.0f] 范围内
  */
fp32 theta_format(fp32 Ang)
{
    return loop_fp32_constrain(Ang, -180.0f, 180.0f);
}

/**
  * @brief          对浮点数进行四舍五入操作
  * @author         冉文治
  * @param          raw 输入的浮点数
  * @retval         返回四舍五入后的整数值
  */
int32_t float_rounding(fp32 raw)
{
    static int32_t integer;
    static fp32 decimal;
    integer = (int)raw;
    decimal = raw - integer;
    if (decimal > 0.5f)
        integer++;
    return integer;
}

/**
  * @brief          初始化普通最小二乘（OLS）算法的相关参数
  * @author         冉文治
  * @param          OLS 指向 `Ordinary_Least_Squares_t` 结构体的指针，用于存储 OLS 的参数和数据
  * @param          order 数据的阶数，决定了 `x` 和 `y` 数组的大小
  * @retval         无返回值，初始化完成后，`OLS` 结构体包含了相应的参数和内存分配
  */
void OLS_Init(Ordinary_Least_Squares_t *OLS, uint16_t order)
{
    OLS->Order = order;
    OLS->Count = 0;
    OLS->x = (fp32 *)user_malloc(sizeof(fp32) * order);
    OLS->y = (fp32 *)user_malloc(sizeof(fp32) * order);
    OLS->k = 0;
    OLS->b = 0;
    memset((void *)OLS->x, 0, sizeof(fp32) * order);
    memset((void *)OLS->y, 0, sizeof(fp32) * order);
    memset((void *)OLS->t, 0, sizeof(fp32) * 4);
}

/**
  * @brief          更新普通最小二乘（OLS）算法的参数，计算回归直线的斜率和截距
  * @author         冉文治
  * @param          OLS 指向 `Ordinary_Least_Squares_t` 结构体的指针，包含当前的 OLS 参数和数据
  * @param          deltax 当前位置的增量（x值的变化量）
  * @param          y 当前的 y 值（对应于 `deltax`）
  * @retval         无返回值，更新 OLS 结构体中的 `k`（斜率）、`b`（截距）以及标准差 `StandardDeviation`
  */
void OLS_Update(Ordinary_Least_Squares_t *OLS, fp32 deltax, fp32 y)
{
    static fp32 temp = 0;
    temp = OLS->x[1];
    for (uint16_t i = 0; i < OLS->Order - 1; ++i)
    {
        OLS->x[i] = OLS->x[i + 1] - temp;
        OLS->y[i] = OLS->y[i + 1];
    }
    OLS->x[OLS->Order - 1] = OLS->x[OLS->Order - 2] + deltax;
    OLS->y[OLS->Order - 1] = y;

    if (OLS->Count < OLS->Order)
    {
        OLS->Count++;
    }
    memset((void *)OLS->t, 0, sizeof(fp32) * 4);
    for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i)
    {
        OLS->t[0] += OLS->x[i] * OLS->x[i];
        OLS->t[1] += OLS->x[i];
        OLS->t[2] += OLS->x[i] * OLS->y[i];
        OLS->t[3] += OLS->y[i];
    }

    OLS->k = (OLS->t[2] * OLS->Order - OLS->t[1] * OLS->t[3]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]);
    OLS->b = (OLS->t[0] * OLS->t[3] - OLS->t[1] * OLS->t[2]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]);

    OLS->StandardDeviation = 0;
    for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i)
    {
        OLS->StandardDeviation += fabsf(OLS->k * OLS->x[i] + OLS->b - OLS->y[i]);
    }
    OLS->StandardDeviation /= OLS->Order;
}

/**
  * @brief          更新普通最小二乘（OLS）算法的斜率（导数），并返回当前斜率值
  * @author         冉文治
  * @param          OLS 指向 `Ordinary_Least_Squares_t` 结构体的指针，包含当前的 OLS 参数和数据
  * @param          deltax 当前位置的增量（x值的变化量）
  * @param          y 当前的 y 值（对应于 `deltax`）
  * @retval         返回当前的斜率（`k`），即线性回归模型的导数
  */
fp32 OLS_Derivative(Ordinary_Least_Squares_t *OLS, fp32 deltax, fp32 y)
{
    static fp32 temp = 0;
    temp = OLS->x[1];
    for (uint16_t i = 0; i < OLS->Order - 1; ++i)
    {
        OLS->x[i] = OLS->x[i + 1] - temp;
        OLS->y[i] = OLS->y[i + 1];
    }
    OLS->x[OLS->Order - 1] = OLS->x[OLS->Order - 2] + deltax;
    OLS->y[OLS->Order - 1] = y;

    if (OLS->Count < OLS->Order)
    {
        OLS->Count++;
    }

    memset((void *)OLS->t, 0, sizeof(fp32) * 4);
    for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i)
    {
        OLS->t[0] += OLS->x[i] * OLS->x[i];
        OLS->t[1] += OLS->x[i];
        OLS->t[2] += OLS->x[i] * OLS->y[i];
        OLS->t[3] += OLS->y[i];
    }

    OLS->k = (OLS->t[2] * OLS->Order - OLS->t[1] * OLS->t[3]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]);

    OLS->StandardDeviation = 0;
    for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i)
    {
        OLS->StandardDeviation += fabsf(OLS->k * OLS->x[i] + OLS->b - OLS->y[i]);
    }
    OLS->StandardDeviation /= OLS->Order;

    return OLS->k;
}

/**
  * @brief          获取当前普通最小二乘（OLS）算法的斜率（导数）
  * @author         冉文治
  * @param          OLS 指向 `Ordinary_Least_Squares_t` 结构体的指针，包含当前的 OLS 参数和数据
  * @retval         返回当前的斜率（`k`），即线性回归模型的导数
  */
fp32 Get_OLS_Derivative(Ordinary_Least_Squares_t *OLS)
{
    return OLS->k;
}

/**
  * @brief          更新普通最小二乘（OLS）算法的参数，计算回归值并平滑输出
  * @author         冉文治
  * @param          OLS 指向 `Ordinary_Least_Squares_t` 结构体的指针，包含当前的 OLS 参数和数据
  * @param          deltax 当前位置的增量（x值的变化量）
  * @param          y 当前的 y 值（对应于 `deltax`）
  * @retval         返回平滑后的回归值
  */
fp32 OLS_Smooth(Ordinary_Least_Squares_t *OLS, fp32 deltax, fp32 y)
{
    static fp32 temp = 0;
    temp = OLS->x[1];
    for (uint16_t i = 0; i < OLS->Order - 1; ++i)
    {
        OLS->x[i] = OLS->x[i + 1] - temp;
        OLS->y[i] = OLS->y[i + 1];
    }
    OLS->x[OLS->Order - 1] = OLS->x[OLS->Order - 2] + deltax;
    OLS->y[OLS->Order - 1] = y;

    if (OLS->Count < OLS->Order)
    {
        OLS->Count++;
    }

    memset((void *)OLS->t, 0, sizeof(fp32) * 4);
    for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i)
    {
        OLS->t[0] += OLS->x[i] * OLS->x[i];
        OLS->t[1] += OLS->x[i];
        OLS->t[2] += OLS->x[i] * OLS->y[i];
        OLS->t[3] += OLS->y[i];
    }

    OLS->k = (OLS->t[2] * OLS->Order - OLS->t[1] * OLS->t[3]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]);
    OLS->b = (OLS->t[0] * OLS->t[3] - OLS->t[1] * OLS->t[2]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]);

    OLS->StandardDeviation = 0;
    for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i)
    {
        OLS->StandardDeviation += fabsf(OLS->k * OLS->x[i] + OLS->b - OLS->y[i]);
    }
    OLS->StandardDeviation /= OLS->Order;

    return OLS->k * OLS->x[OLS->Order - 1] + OLS->b;
}

/**
  * @brief          获取当前普通最小二乘（OLS）算法的平滑输出值
  * @author         冉文治
  * @param          OLS 指向 `Ordinary_Least_Squares_t` 结构体的指针，包含当前的 OLS 参数和数据
  * @retval         返回当前平滑后的回归值
  */
fp32 Get_OLS_Smooth(Ordinary_Least_Squares_t *OLS)
{
    return OLS->k * OLS->x[OLS->Order - 1] + OLS->b;
}
